import RPi.GPIO as GPIO
import time
# bgr8转jpeg格式
import enum
import cv2
import ipywidgets.widgets as widgets

# 线程功能操作库
import threading
import inspect
import ctypes
import numpy as np
from recogn import *


# 线程相关函数
def _async_raise(tid, exctype):
    """raises the exception, performs cleanup if needed"""
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        # """if it returns a number greater than one, you're in trouble,
        # and you should call it again with exc=NULL to revert the effect"""
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)


def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)

def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])

origin_widget = widgets.Image(format='jpeg', width=320, height=240)
mask_widget = widgets.Image(format='jpeg',width=320, height=240)
result_widget = widgets.Image(format='jpeg',width=320, height=240)
image_container = widgets.HBox([origin_widget, mask_widget, result_widget])

def recognize_color():

    global color_recognize

    cap = cv2.VideoCapture(0)
    cap.set(3, 640)
    cap.set(4, 480)
    cap.set(5, 120)
    cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
    cap.set(cv2.CAP_PROP_BRIGHTNESS, 20)
    cap.set(cv2.CAP_PROP_CONTRAST, 20)

    red_lower = np.array([0, 43, 46])
    red_upper = np.array([10, 255, 255])
    blue_lower = np.array([100, 43, 46])
    blue_upper = np.array([124, 255, 255])

    while True:
        ret, frame = cap.read()
        cv2.imshow('Video', frame)
        origin_widget.value = bgr8_to_jpeg(frame)

        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        mask1 = cv2.inRange(hsv, blue_lower, blue_upper)
        mask2 = cv2.inRange(hsv, red_lower, red_upper)
        mask_widget.value = bgr8_to_jpeg(mask1)
        mask_widget.value = bgr8_to_jpeg(mask2)

        if cv2.countNonZero(mask1) > 10000:
            color_recognize = 1
            print('蓝方')
            break
        elif cv2.countNonZero(mask2) > 20000:
            color_recognize = 2
            print('红方')
            break

    cap.release()
    cv2.destroyAllWindows()


class ShapeDetector:
    def __init__(self):
        pass

    # 轮廓形状识别器 只有一个参数 c：轮廓
    # 为了进行形状检测，我们将使用轮廓近似法。 顾名思义，轮廓近似（contour approximation）是一种算法，用于通过减少一组点来减少曲线中的点数，因此称为术语近似。
    # 轮廓近似是基于以下假设：一条曲线可以由一系列短线段近似。这将导致生成近似曲线，该曲线由原始曲线定义的点子集组成。
    # 轮廓近似实际上已经通过cv2.approxPolyDP在OpenCV中实现。
    def detect(self, c):
        # 初始化形状名称，使用轮廓近似法
        # 计算周长
        peri = cv2.arcLength(c, True)
        approx = cv2.approxPolyDP(c, 0.04 * peri, True)

        # 轮廓是由一系列顶点组成的；如果是三角形，将拥有3个向量
        if len(approx) == 3:
            shape = "triangle"
        # 如果有4个顶点，那么是矩形或者正方形
        elif len(approx) == 4:
            # 计算轮廓的边界框 并且计算宽高比
            (x, y, w, h) = cv2.boundingRect(approx)
            ar = w / float(h)
            # 正方形的宽高比~~1 ，否则是矩形
            shape = "square" if 0.95 <= ar <= 1.05 else "rectangle"
        # 否则，根据上边的膨胀腐蚀，我们假设它为圆形
        else:
            shape = "circle"
        # 返回形状的名称
        return shape


# ser = serial.Serial("/dev/ttyS0", 9600, timeout=0.1)

# red1
r_low_hsv1 = np.array([160, 43, 60])
r_high_hsv1 = np.array([180, 255, 255])
# red2
r_low_hsv2 = np.array([0, 43, 60])
r_high_hsv2 = np.array([10, 255, 255])
# green
g_low_hsv1 = np.array([50, 43, 60])
g_high_hsv1 = np.array([94, 255, 255])
# blue
b_low_hsv1 = np.array([100, 43, 60])
b_high_hsv1 = np.array([124, 255, 255])
# yellow
y_low_hsv1 = np.array([11, 43, 60])
y_high_hsv1 = np.array([31, 255, 255])
font = cv2.FONT_HERSHEY_SIMPLEX


def image_tf_judge(img):

    global is_detected

    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # mask是只突出指定颜色的图片
    mask_red = cv2.inRange(hsv, lowerb=r_low_hsv1, upperb=r_high_hsv1) + cv2.inRange(hsv, lowerb=r_low_hsv2,
                                                                                     upperb=r_high_hsv2)
    mask_blue = cv2.inRange(hsv, lowerb=b_low_hsv1, upperb=b_high_hsv1)
    mask_yellow = cv2.inRange(hsv, lowerb=y_low_hsv1, upperb=y_high_hsv1)
    mask_green = cv2.inRange(hsv, lowerb=g_low_hsv1, upperb=g_high_hsv1)

    inner_mask = [mask_yellow, mask_green]
    inner_mask_S = []
    inner_mask_counter = []
    outer_mask = [mask_blue + mask_yellow + mask_green, mask_red + mask_yellow + mask_green]
    outer_mask_S = []
    outer_mask_counter = []
    features = {"inner_color": "none", "inner_shape": "none", "outer_color": "none"}  # 内部颜色 内部形状 外部颜色 外部形状
    sd = ShapeDetector()
    detect = 0
    for i in range(len(inner_mask)):
        # 中值滤波降噪
        median = cv2.medianBlur(inner_mask[i], 7)
        k1 = np.ones((5, 5), np.uint8)
        opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, k1)
        contours, hierarchy = cv2.findContours(median, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if len(contours) != 0:
            area = []
            # 找到最大的轮廓
            for k in range(len(contours)):
                area.append(cv2.contourArea(contours[k]))
            max_idx = np.argmax(np.array(area))
            inner_mask_S.append(area[max_idx])
            inner_mask_counter.append(contours[max_idx])
        else:
            inner_mask_S.append(0)
            inner_mask_counter.append(None)
    inner_tag = inner_mask_S.index(max(inner_mask_S))
    if inner_tag == 0:
        features["inner_color"] = "yellow"
    else:
        features["inner_color"] = "green"
    if inner_mask_counter[inner_tag] is not None:
        shape = sd.detect(inner_mask_counter[inner_tag])
        if shape == "triangle":
            features["inner_shape"] = "triangle"
        elif shape == "circle":
            features["inner_shape"] = "circle"

    for i in range(len(outer_mask)):
        # 中值滤波降噪
        median = cv2.medianBlur(outer_mask[i], 7)
        k1 = np.ones((5, 5), np.uint8)
        opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, k1)
        closing = cv2.morphologyEx(opening, cv2.MORPH_CLOSE, k1)
        contours, hierarchy = cv2.findContours(median, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if len(contours) != 0:
            area = []
            # 找到最大的轮廓
            for k in range(len(contours)):
                area.append(cv2.contourArea(contours[k]))
            max_idx = np.argmax(np.array(area))
            outer_mask_S.append(area[max_idx])
            outer_mask_counter.append(contours[max_idx])
        else:
            outer_mask_S.append(0)
            outer_mask_counter.append(None)
    outer_tag = outer_mask_S.index(max(outer_mask_S))
    if outer_tag == 0:
        features["outer_color"] = "blue"
    else:
        features["outer_color"] = "red"
    if inner_mask_counter[inner_tag] is not None:
        M = cv2.moments(inner_mask_counter[inner_tag])  # 求矩
        if M['m00'] != 0:
            cx = int(M['m10'] / M['m00'])  # 求x坐标
            cy = int(M['m01'] / M['m00'])  # 求y坐标
            x, y, w, h = cv2.boundingRect(outer_mask_counter[outer_tag])
            if x < cx < x + w and y < cy < y + w:
                if 0.05 < (inner_mask_S[inner_tag] / outer_mask_S[outer_tag]) < 0.3:
                    #蓝真
                    if features["inner_color"] == "yellow" and features["inner_shape"] == "circle" and features[
                        "outer_color"] == "blue":
                        detect = 1
                    #蓝假
                    elif features["inner_color"] == "green" and features["inner_shape"] == "triangle" and features[
                        "outer_color"] == "blue":
                        detect = 2
                    #红真
                    elif features["inner_color"] == "yellow" and features["inner_shape"] == "circle" and features[
                        "outer_color"] == "red":
                        detect = 3
                    #红假
                    elif features["inner_color"] == "green" and features["inner_shape"] == "triangle" and features[
                        "outer_color"] == "red":
                        detect = 4

    cv2.imshow("Video", frame)
    #如果是蓝方
    if color_recognize == 1:
        if detect == 1:
            is_detected = 1
        if detect == 2 or detect == 3 or detect == 4:
            is_detected = 0
    #如果是红方
    if color_recognize == 2:
        if detect == 3:
            is_detected = 1
        if detect == 1 or detect == 2 or detect == 4:
            is_detected = 0

    return is_detected


# 小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13

# 小车按键定义
key = 8

# 循迹红外引脚定义
# TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
#      3                 5                  4                   18
TrackSensorLeftPin1 = 3  # 定义左边第一个循迹红外传感器引脚为3口
TrackSensorLeftPin2 = 5  # 定义左边第二个循迹红外传感器引脚为5口
TrackSensorRightPin1 = 4  # 定义右边第一个循迹红外传感器引脚为4口
TrackSensorRightPin2 = 18  # 定义右边第二个循迹红外传感器引脚为18口

# 设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)

# 忽略警告信息
GPIO.setwarnings(False)

# 电机引脚初始化为输出模式
# 按键引脚初始化为输入模式
# 寻迹引脚初始化为输入模式
def init():
    global pwm_ENA
    global pwm_ENB
    GPIO.setup(ENA, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(ENB, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(key, GPIO.IN)
    GPIO.setup(TrackSensorLeftPin1, GPIO.IN)
    GPIO.setup(TrackSensorLeftPin2, GPIO.IN)
    GPIO.setup(TrackSensorRightPin1, GPIO.IN)
    GPIO.setup(TrackSensorRightPin2, GPIO.IN)
    # 设置pwm引脚和频率为2000hz
    pwm_ENA = GPIO.PWM(ENA, 2000)
    pwm_ENB = GPIO.PWM(ENB, 2000)
    pwm_ENA.start(0)
    pwm_ENB.start(0)

# 小车前进
def run(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

# 小车后退
def back(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

# 小车左转
def left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

# 小车右转
def right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

def runone(delaytime):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)

def backone(delaytime):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)

# 小车原地左转
def spin_left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

# 小车原地右转
def spin_right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

# 小车停止
def brake():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)

# 按键检测
def key_scan():
    while GPIO.input(key):
        pass
    while not GPIO.input(key):
        time.sleep(0.01)
        if not GPIO.input(key):
            time.sleep(0.01)
        while not GPIO.input(key):
            pass

time.sleep(2)

# 扫描
init()
key_scan()
print('first key')
# cap1 = cv2.VideoCapture(0)
all_directions = detect()
print(all_directions)
# 扫描

# try/except语句用来检测try语句块中的错误，
# 从而让except语句捕获异常信息并处理。

try:
    # init()
    key_scan()
    recognize_color()
    key_scan()
    i = 0
    while (i <= len(all_directions)):
        total_crossroads = len(all_directions[i])
        current_crossroad = 0
        while current_crossroad < total_crossroads:
            while True:
                TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                # 循迹部分
                if (TrackSensorLeftValue1 == False or TrackSensorRightValue2 == False):
                    break
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                    run(20, 20)
                    # 路口判断
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                    left(0, 35)
                elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                    right(35, 0)
            start_time = time.time()
            # 路口转弯
            while True:
                TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                if (all_directions[i][current_crossroad] == '1'):
                    if TrackSensorLeftValue1 == False:
                        spin_left(50, 50)  # 40
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time1 = time.time()
                    if current_time1 - start_time >= 0.9:
                        break
                elif (all_directions[i][current_crossroad] == '0'):
                    if TrackSensorRightValue2 == False:
                        spin_right(50, 50)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time2 = time.time()
                    if current_time2 - start_time >= 0.9:
                        break

                elif (all_directions[i][current_crossroad] == '3'):
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time3 = time.time()
                    if current_time3 - start_time >= 0.3:
                        break
            current_crossroad += 1
        # 宝藏识别
        print('进入宝藏识别')
        # 创建视频捕获器
        cap = cv2.VideoCapture(0)
        # 检查捕获器是否成功打开
        if not cap.isOpened():
            print("无法打开摄像头")
            exit()
        # 读取和处理视频帧
        while True:
            while True:
                print('降速循迹寻找宝藏')
                while True:
                    TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(5, 5)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)

                    if cap.isOpened():
                        while True:
                            ret, frame = cap.read()
                            image_tf_judge(frame)
                            if is_detected == 1:
                                print("真宝藏")
                                break
                            elif is_detected == 0:
                                print("假宝藏")
                                break

                    if ((is_detected == 1) or (is_detected == 0)):
                        break

                if is_detected == 1:
                    runone(0.2)
                    backone(0.4)
                    start_time1_1 = time.time()
                    while True:
                        TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                        TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                        spin_left(40, 40)
                        current_time1_1 = time.time()
                        if current_time1_1 - start_time1_1 >= 0.87:
                            break
                    start_time1_2 = time.time()
                    while True:
                        TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                        TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                        if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                            run(7, 7)  # 14
                        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                            left(0, 35)
                        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                            right(35, 0)
                        current_time1_2 = time.time()
                        if current_time1_2 - start_time1_2 >= 0.4:
                            break
                elif is_detected == 0:
                    start_time1_1 = time.time()
                    backone(0.1)
                    while True:
                        TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                        TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                        spin_left(40, 40)
                        current_time1_1 = time.time()
                        if current_time1_1 - start_time1_1 >= 0.87:
                            break
                    start_time1_2 = time.time()
                    while True:
                        TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                        TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                        if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                            run(5, 5)  # 14
                        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                            left(0, 35)
                        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                            right(35, 0)
                        current_time1_2 = time.time()
                        if current_time1_2 - start_time1_2 >= 0.4:
                            break
                break
        cap.release()
        cv2.destroyAllWindows()
        i = i + 1

except KeyboardInterrupt:
    pass
pwm_ENA.stop()
pwm_ENB.stop()
GPIO.cleanup()